
#ifndef _ARTIFICIALBONE_H_
#define _ARTIFICIALBONE_H_

#include "gskeleton.h"
#include <queue>
using namespace std;


struct compareDegree {
   bool operator() (const pair<int,int> &a, const pair<int,int> &b) const {
      return a.second > b.second;
   }
};

class ArtificialBone {
   
public:
   ArtificialBone(GSkeleton *source, GSkeleton *target) {
      
      const vector<pair<int, int> > &bones = source->bones;
      vector<vector<int> > adjBones(source->nJoints());
      for (int i=0; i<bones.size(); ++i) {
         adjBones[bones[i].first].push_back(i);
         adjBones[bones[i].second].push_back(i);
      }
      
      vector<int> sizeBonesFirst(source->nBones());
      // build the graph of bones
      vector<vector<int> > graphBones(source->nBones());
      for (int i=0; i<source->nBones(); ++i) {
         int bonesFirst = bones[i].first;
         for (int j=0; j<adjBones[bonesFirst].size(); ++j) {
            if (i != adjBones[bonesFirst][j])
               graphBones[i].push_back(adjBones[bonesFirst][j]);
         }
         sizeBonesFirst[i] = graphBones[i].size();
         int bonesSecond = bones[i].second;
         for (int j=0; j<adjBones[bonesSecond].size(); ++j) {
            if (i != adjBones[bonesSecond][j])
               graphBones[i].push_back(adjBones[bonesSecond][j]);
         }
      }
      
      /// 
      vector<int> firstReached(source->nBones());
      vector<bool> visited(source->nJoints(), false);
      // pair<i,j> : i: idbone, j=degree the first reached of bone
      priority_queue<pair<int, int>, vector<pair<int, int> >, compareDegree> pqueue;
      
      for (int i=0; i<source->nBones(); ++i) {
         if (source->jointDegree(bones[i].first) == 1) {
            firstReached[i] = bones[i].first;
            pqueue.push(make_pair(i,1));
         }
         else if (source->jointDegree(bones[i].second) == 1) {
            firstReached[i] = bones[i].second;
            pqueue.push(make_pair(i,1));
         }
      }
      
      source->controller.clear();
      float LIMINF = 0.1;
      
      while (not pqueue.empty()) {
         pair<int, int> boneIdDegree = pqueue.top();
         pqueue.pop();
         int boneid = boneIdDegree.first;
         visited[boneid] = true;
         int initialNeighbor = 0, terminalNeighbor = graphBones[boneid].size();
         int expansionJoint = bones[boneid].first;
         if (firstReached[boneid] == bones[boneid].first) {
            expansionJoint = bones[boneid].second;
            initialNeighbor = sizeBonesFirst[boneid];
         } else 
            terminalNeighbor = sizeBonesFirst[boneid];
         
         const vector<int> &adj = graphBones[boneid];
         for (int i=initialNeighbor; i<terminalNeighbor; ++i) {
            firstReached[adj[i]] = expansionJoint;
            if (not visited[adj[i]]) {
               int degree; int otherJunction = 0;
               if (bones[adj[i]].first == expansionJoint) {
                  degree = source->jointDegree(bones[adj[i]].second);
                  otherJunction = 1;
               }
               else 
                  degree = source->jointDegree(bones[adj[i]].first);
               
               if (not visited[adj[i]])
                  pqueue.push(make_pair(adj[i], degree));
               
               if (degree == 2) {
                  if (boneIdDegree.second == 1) {
                     Vector3 cross = cross_product(source->getBoneUnitVector(boneid),
                                                   source->getBoneUnitVector(adj[i]));
                     if (cross*cross>LIMINF) 
                        source->controller[boneid].push_back(adj[i]);
                  }
                  source->controller[adj[i]].push_back(boneid);
               }
               if (degree == 3) {
                  int in, tn;
                  if (otherJunction==0) {
                     in = 0; tn = sizeBonesFirst[adj[i]];
                  }
                  else {
                     in = sizeBonesFirst[adj[i]]; tn = graphBones[adj[i]].size();
                  }
                  for (int ii = in; ii<tn; ii++) {
                     int cbone = graphBones[adj[i]][ii];
                     Vector3 cross = cross_product(source->getBoneUnitVector(cbone),
                                                   source->getBoneUnitVector(adj[i]));
                     if (cross*cross>LIMINF) 
                        source->controller[boneid].push_back(adj[i]);
                  }
               }
               if (degree > 3) {
                  int in, tn;
                  if (otherJunction==0) {
                     in = 0; tn = sizeBonesFirst[adj[i]];
                  }
                  else {
                     in = sizeBonesFirst[adj[i]]; tn = graphBones[adj[i]].size();
                  }
                  float norma_cross[2]={-1e8,-1e8};  int indmin = 0; int bone_temp[2];
                  for (int ii = in; ii<tn; ii++) {
                     int cbone = graphBones[adj[i]][ii];
                     Vector3 crosstmp = cross_product(source->getBoneUnitVector(cbone),
                                                      source->getBoneUnitVector(adj[i]));
                     float norma = crosstmp*crosstmp;
                     if (norma > norma_cross[indmin]) {
                        norma_cross[indmin] = norma; 
                        bone_temp[indmin] = adj[i];
                        if(norma > norma_cross[1-indmin])
                           indmin = 1-indmin;
                     }
                  }
                  source->controller[boneid].push_back(bone_temp[0]);
                  source->controller[boneid].push_back(bone_temp[1]);
               }
            }
         } //
      }
      
      cout<<"controllers ..."<<endl;
      for (int i=0; i<source->controller.size(); ++i) {
         const vector<int> &con = source->controller[i];
         cout<<"c: "<<i<<" : ";
         for (int j=0; j<con.size(); ++j) {
            cout<<con[j]<<" ";
         }
         cout<<endl;
      }
   }
};

#endif
